#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <cmath>

class ImuBehavior : public rclcpp::Node
{
public:
    ImuBehavior() : Node("imu_behavior")
    {
        imu_sub = this->create_subscription<sensor_msgs::msg::Imu>(
            "/imu/data", 10, std::bind(&ImuBehavior::imu_callback, this, std::placeholders::_1));
        twist_pub = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
        target_yaw = M_PI / 2;
    }

private:
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub;
    double target_yaw;
};

void ImuBehavior::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
{
    geometry_msgs::msg::Twist twist;
    double roll, pitch, yaw;
    tf2::Quaternion Quaternion(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
    tf2::Matrix3x3(Quaternion).getRPY(roll, pitch, yaw);

    RCLCPP_INFO(this->get_logger(), "Roll: %f, Pitch: %f, Yaw: %f", roll, pitch, yaw);


    // Calculate the error between the current yaw and the target yaw
    double error = target_yaw - yaw;

    // Calculate the angular velocity to achieve the target yaw
    double angular_velocity = error * 0.5;

    // Publish the angular velocity to the cmd_vel topic
    twist.angular.z = angular_velocity;
    twist_pub->publish(twist);

}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImuBehavior>());
    rclcpp::shutdown();
    return 0;
}